#include <joyos.h>
#include <Math.h>
#include "strategy.h"
#include "navigate5.h"
#include "gather.h"
#include "deposit.h"


uint16_t territoryChoice[6] = {0, 0, 0, 0, 0, 0};


void exploreTerritories(){
	uint8_t myTerritory = inSection(getMyPosition());
	uint8_t newTerritory = (myTerritory +1) % 6;
	int startTime = get_time();
	
	while((get_time() - startTime) <= 10000){
		myTerritory = inSection(getMyPosition());
		newTerritory = (myTerritory +1) % 6;
		
		if (newTerritory == 0){
			newTerritory = 6;
		}
		
		driveToPoint(getSection(newTerritory), 3.0/12.);
	}
	
	motor_set_vel(0, 0);
	motor_set_vel(1, 0);
}

uint8_t chooseNextTerritory(){
	uint8_t myTerritory = inSection(getMyPosition()) - 1;
	uint8_t opponentTerritory = inSection(getOpponentPosition()) - 1;
	uint8_t bestTerritory = (myTerritory + 1) % 6;
	
	uint8_t sheepLeft = getTerritorySheepLeft(myTerritory);	
	uint16_t sheepTimeLeft = getTerritorySheepTimeLeft(myTerritory);	
	
	for(uint8_t i = 0; i< 6; i++){
		sheepLeft = getTerritorySheepLeft(i + 1);
		sheepTimeLeft = getTerritorySheepTimeLeft(i + 1);
		
		
		if (i == opponentTerritory){
			territoryChoice[i] = 0;
		}else if (sheepLeft < 1 && sheepTimeLeft > 15){
			territoryChoice[i] = 1;
		}else if (fabs(i - myTerritory) == 3){
			territoryChoice[i] = 2;
		}else if (fabs(i - myTerritory) >1 && fabs(i-myTerritory) <5){
			if (((myTerritory + 2) % 6 == i && (myTerritory + 1) % 6 == opponentTerritory) || 
				((myTerritory - 2 + 6) % 6 == i && (myTerritory - 1 + 6) % 6 == opponentTerritory)){
				territoryChoice[i] = 1;
			}else{
				territoryChoice[i] = 4;
			}
			
		}else if ((i + 1) % 6 == opponentTerritory || (i - 1 + 6) % 6 == opponentTerritory){
			territoryChoice[i] = 5;
		}else{
			territoryChoice[i] = 6;
		}
	}
	
	for (uint8_t i = 0; i<6; i++){
		if (territoryChoice[i] > territoryChoice[bestTerritory]){
			bestTerritory = i;
		}
	}
	
	return bestTerritory + 1;
}

void closestUnminedTerritory(){
	uint8_t myTerritory = inSection(getMyPosition());
	if (sheepLeft())
}

